﻿using OpenTK;

namespace AnimatingHair.Entity.PhysicalEntity
{
    abstract class PhysicalEntity
    {
        public double Mass { get; set; }

        public Vector3d Force;
        public Vector3d Acceleration;
        public Vector3d Velocity;
        public Vector3d Position;

        #region Runge-Kutta members

        private Vector3d[] kVelocity = new Vector3d[ 5 ];
        private Vector3d[] kPosition = new Vector3d[ 5 ];
        private Vector3d startVelocity;
        private Vector3d startPosition;

        #endregion

        #region Runge-Kutta methods

        internal virtual void RKStep( int stepNumber )
        {
            Acceleration = Force / Mass;

            switch ( stepNumber )
            {
                case 0:
                    RKInit();
                    break;

                case 1:
                    RKStep1();
                    break;

                case 2:
                    RKStep2();
                    break;

                case 3:
                    RKStep3();
                    break;

                case 4:
                    RKStep4();
                    break;

                case 5:
                    RKFinalize();
                    break;
            }
        }

        private void RKInit()
        {
            startVelocity = Velocity;
            startPosition = Position;
        }

        private void RKStep1()
        {
            kVelocity[ 1 ] = Acceleration * Const.TimeStep;
            kPosition[ 1 ] = startVelocity * Const.TimeStep;

            // priprava na k2:
            Position = startPosition + kPosition[ 1 ] / 2;
            Velocity = startVelocity + kVelocity[ 1 ] / 2;
        }

        private void RKStep2()
        {
            kVelocity[ 2 ] = Acceleration * Const.TimeStep;
            //kPosition[ 2 ] = Velocity * Const.TimeStep; TODO: je to ekvivalentne tomu pod tym?
            kPosition[ 2 ] = (startVelocity + kVelocity[ 1 ] / 2) * Const.TimeStep;

            // priprava na k3:
            Position = startPosition + kPosition[ 2 ] / 2;
            Velocity = startVelocity + kVelocity[ 2 ] / 2;
        }

        private void RKStep3()
        {
            kVelocity[ 3 ] = Acceleration * Const.TimeStep;
            //kPosition[ 3 ] = Velocity * Const.TimeStep;
            kPosition[ 3 ] = (startVelocity + kVelocity[ 2 ] / 2) * Const.TimeStep;

            // priprava na k3:
            Position = startPosition + kPosition[ 3 ];
            Velocity = startVelocity + kVelocity[ 3 ];
        }

        private void RKStep4()
        {
            kVelocity[ 4 ] = Acceleration * Const.TimeStep;
            //kPosition[ 4 ] = Velocity * Const.TimeStep;
            kPosition[ 4 ] = (startVelocity + kVelocity[ 3 ]) * Const.TimeStep;
        }

        private void RKFinalize()
        {
            Velocity = startVelocity + (1.0 / 6.0) * (kVelocity[ 1 ] + 2 * (kVelocity[ 2 ] + kVelocity[ 3 ]) + kVelocity[ 4 ]);
            Position = startPosition + (1.0 / 6.0) * (kPosition[ 1 ] + 2 * (kPosition[ 2 ] + kPosition[ 3 ]) + kPosition[ 4 ]);
        }

        #endregion
    }
}